#include<ros/ros.h>
#include<tf2_ros/transform_listener.h>
#include<tf2_ros/buffer.h>
#include<geometry_msgs/PoseStamped.h>
#include<tf2_geometry_msgs/tf2_geometry_msgs.h>
#include<geometry_msgs/TransformStamped.h>
/*
    计算son1 与 son2的相对关系 计算son1的中某个坐标点在son2 中的坐标值


*/



int main(int argc, char *argv[])
{

    setlocale(LC_ALL,"");


    ros::init(argc,argv,"tfs_sub");

    ros::NodeHandle nh;

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);


    geometry_msgs::PointStamped psAtson1;
    psAtson1.header.frame_id = "son1";
    psAtson1.header.stamp = ros::Time::now();

    psAtson1.point.x = 1.0;
    psAtson1.point.y = 2.0;
    psAtson1.point.z = 3.0;



    ros::Rate rate(1);

    while (ros::ok())
    {
        try{ 
            geometry_msgs::TransformStamped son1Toson2 = buffer.lookupTransform("son2","son1",ros::Time(0));  
            ROS_INFO("son1 相对于 son2 的信息: 父级 ：%s  , 子级： %s, 偏移量(%.2f %.2f %.2f)",son1Toson2.header.frame_id.c_str(),
            son1Toson2.child_frame_id.c_str(),
            son1Toson2.transform.translation.x,
            son1Toson2.transform.translation.y,
            son1Toson2.transform.translation.z); 
            geometry_msgs::PointStamped psAtson2 =  buffer.transform(psAtson1,"son2");
            ROS_INFO("坐标点在 son2 中的值(%.2f %.2f %.2f)",psAtson2.point.x,psAtson2.point.y,psAtson2.point.z);
            rate.sleep();
        }
        catch(const std::exception& e){
            ROS_INFO("错误提示：%s",e.what());
        }      

    }
    
    return 0;
}
